#include "IOManager.h"
#include <fstream>
#include <iostream>
#include <math.h>
#include <algorithm>



IOManager::IOManager()
{
}

void IOManager::ReadDataToDatabase(string _fileName)
{
	//read from file line by line
	char buffer[256];
	ifstream offFile (_fileName.c_str());
	
	offFile.getline (buffer,100);
	string firtsLine = string(buffer);
	if (firtsLine.compare("OFF") == 0)
	{
        cout << "PROCESSING. . . . ." << endl;
		offFile.getline(buffer, 100);
		string secondLine = string(buffer);
		//cut string
		int fisrtSpace = secondLine.find_first_of(" ");
		int secondSpace = secondLine.find_last_of(" ");
		//get individual data
		int numVertex = atoi(secondLine.substr(0, fisrtSpace).c_str());
		int numFace = atoi (secondLine.substr(fisrtSpace + 1, secondSpace - fisrtSpace - 1).c_str());
		//process vertex
		for (int i = 0; i < numVertex; ++i)
		{
			offFile.getline(buffer, 100);
			string vertexCoor = string(buffer);
			int first = vertexCoor.find_first_of(" ");
			int last = vertexCoor.find_last_of(" ");

			double x = atof(vertexCoor.substr(0, first).c_str());
			double y = atof(vertexCoor.substr(first + 1, last - first - 1).c_str());
			double z = atof(vertexCoor.substr(last + 1, vertexCoor.length() - last - 1).c_str());
			l_VertexDataArray.push_back(Vertex(x, y, z));
		}
		//process faces
		for (int i = 0; i < numFace; ++i)
		{
			offFile.getline(buffer, 100);
			string faceData = string(buffer);
			int currIndex = 0;
			int oldIndex = -1;

			Face m_face(-1);

			while (currIndex <= faceData.length())
			{
				if (currIndex == faceData.length() || faceData.at(currIndex) == ' ')
				{
					int temp = atoi(faceData.substr(oldIndex + 1, currIndex - oldIndex - 1).c_str());
					if (m_face.numVertex == -1)
					{
						m_face.numVertex = temp;
					}
					else
					{
						m_face.listVertexIndex.push_back(temp);
					}
					oldIndex = currIndex;
				} 
				else
				{
					//do nothing
				}
				++currIndex;
			}
            GetEdgeDataFromFace(&m_face);
			l_FaceDataArray.push_back(m_face);
		}
        cout << "DONE" << endl;
	}
	else
	{
		cout << "File corrupted" << endl;
	}
}

void IOManager::CalculateUnfoldAngle()
{
    //
    for (int i = 0; i < l_VertexDataArray.size(); ++i)
    {
        Vertex &currVertex = l_VertexDataArray.at(i);
        Edge* currEdge = FindEdgeByStartPoint(i);
        int currIndex = -1;
        int start = -1;
        if (currEdge->vertex1Index == i)
        {
            start = currEdge->vertex2Index;
        }
        else
        {
            start = currEdge->vertex1Index;
        }
        while (currIndex == -1 || currIndex != start)
        {
            int neighborIndex = -1;
            if (currEdge->listNeighbor.at(0) == currIndex)
            {
                neighborIndex = currEdge->listNeighbor.at(1);
            } 
            else
            {
                neighborIndex = currEdge->listNeighbor.at(0);
            }
            Edge* otherEdge = FindEdge(i, neighborIndex);
            currVertex.unfoldAngle += CalculateAngleBetween(currVertex, *currEdge, *otherEdge);
            //update
            if (currEdge->vertex1Index == i)
            {
                currIndex = currEdge->vertex2Index;
            }
            else
            {
                currIndex = currEdge->vertex1Index;
            }
            currEdge = otherEdge;
        }
        //
    }
}

double IOManager::CalculateAngleBetween(Vertex& commonVertex, Edge& e1, Edge& e2)
{
    //a
    Vertex &v1 = l_VertexDataArray.at(e1.vertex1Index);
    Vertex &v2 = l_VertexDataArray.at(e1.vertex2Index);
    double lengthE1square = (v1.x - v2.x) * (v1.x - v2.x)
                            + (v1.y - v2.y) * (v1.y - v2.y)
                            + (v1.z - v2.z) * (v1.z - v2.z);
    //b
    Vertex &v3 = l_VertexDataArray.at(e2.vertex1Index);
    Vertex &v4 = l_VertexDataArray.at(e2.vertex2Index);
    double lengthE2square = (v3.x - v4.x) * (v3.x - v4.x)
        + (v3.y - v4.y) * (v3.y - v4.y)
        + (v3.z - v4.z) * (v3.z - v4.z);
    //c
    Vertex *v5, *v6;
    if (&v1 == &commonVertex)
    {
        v5 = &v2;
    } 
    else
    {
        v5 = &v1;
    }
    if (&v3 == &commonVertex)
    {
        v6 = &v4;
    } 
    else
    {
        v6 = &v3;
    }
    double lengthE3square = (v5->x - v6->x) * (v5->x - v6->x)
        + (v5->y - v6->y) * (v5->y - v6->y)
        + (v5->z - v6->z) * (v5->z - v6->z);
    //cos acb = a2 + b2 - c2 / 2ab
    double cosAlpha = (lengthE1square + lengthE2square - lengthE3square)
        / (2 * sqrt(lengthE1square) * sqrt(lengthE2square));
    double result = acos(cosAlpha);
    return result;
}

void IOManager::CalculateFunctionValue()
{
	for (int i = 0; i < l_VertexDataArray.size(); ++i)
	{
		Vertex* v = &l_VertexDataArray.at(i);
		v->setVertexFunctionValue(v->y);
	}
}

void IOManager::SetNeighborList()
{
    for (int i = 0; i < l_FaceDataArray.size(); ++i)
    {
        Face* f = &l_FaceDataArray.at(i);
        for (int j = 0; j < f->numVertex; ++j)
        {
            Vertex* v = &l_VertexDataArray.at(f->listVertexIndex.at(j));
            for (int k = 0; k < f->numVertex; ++k)
            {
                if (j != k 
                    && !CheckDuplicate(&v->listNeighbor, f->listVertexIndex.at(k)))
                {
                    v->listNeighbor.push_back(f->listVertexIndex.at(k));
                }
            }
        }
    }
}

bool IOManager::CheckDuplicate(vector<int> * _list, int _key)
{
    if (_list->size() == 0)
    {
        return false;
    }
    for (int i = 0; i < _list->size(); ++i)
    {
        if (_list->at(i) == _key)
        {
            return true;
        }
    }
    return false;
}

void IOManager::CalculateLocalMaxMin()
{
    for (int i = 0; i < l_VertexDataArray.size(); ++i)
    {
        Vertex* v = &l_VertexDataArray.at(i);
        bool canMax = false;
        bool canMin = false;
        for (int j = 0; j < v->listNeighbor.size(); ++j)
        {
            Vertex *neighbor = &l_VertexDataArray.at(v->listNeighbor.at(j));
            if (v->functionValue > neighbor->functionValue)
            {
                canMax = true;
            }
            else if (v->functionValue < neighbor->functionValue)
            {
                canMin = true;
            }
            else
            {
                canMax = true;
                canMin = true;
            }
        }
        //
        if (canMax && !canMin)
        {
            v->isLocalMax = true;
            l_LocalMaxIndex.push_back(i);
        }
        else if (canMin && !canMax)
        {
            v->isLocalMin = true;
            l_LocalMinIndex.push_back(i);
        }
    }
}

void IOManager::CalculateLocalSaddle()
{
	//for each point
	for (int i = 0; i < l_VertexDataArray.size(); ++i)
	{
		//current processed edge
		//choose 1 random edge of this point
		Edge* e = FindEdgeByStartPoint(i);
		//go to 1 direction, compare the value of end point
		
		//		
		int first = -1;
		int second = -1;
		if (e == NULL)
			continue;
		if (i != e->vertex1Index)
		{
			first = e->vertex1Index;
		} 
		else
		{
			first = e->vertex2Index;
		}
		int start = first;
		int lastPoint = first;
		int countChange = 0;
        double origVal = l_VertexDataArray.at(i).functionValue;
        int bIncrease = -1;
		while (true)
		{
			if (e->listNeighbor.size() < 2)
			{
				break;
			}
			if (e->listNeighbor.at(0) != lastPoint)
			{
				second = e->listNeighbor.at(0);
			}
			else
			{
				second = e->listNeighbor.at(1);
			}
			//compare
			double val = l_VertexDataArray.at(first).functionValue;
			if (origVal > val)
			{
				if (bIncrease != 0)
				{
					countChange ++;
				}
				bIncrease = 0;
			} 
			else if (origVal < val)
			{
				if (bIncrease != 1)
				{
					countChange ++;
				}
				bIncrease = 1;
			}
			lastPoint = first;
			first = second;
			second = -1;
			if (first == start)
			{
				break;
			}
			Edge * tempEdge;
			if (i >= first)
			{
				tempEdge = &Edge (first, i);
			}
			else
			{
				tempEdge = &Edge (i, first);
			}
			vector<Edge>::iterator it;
			it = find(l_EdgeDataArray.begin(), l_EdgeDataArray.end(), tempEdge);
			e = &l_EdgeDataArray.at(it - l_EdgeDataArray.begin());
		}

		if (countChange >= 4)
		{
			l_LocalSaddleIndex.push_back(i);
		}
		//if val increase then mark bIncrease = true;
		//else bIncrease = false;
		//each time the bIncrease change, increase count
		//if count = 4 -> saddle point
		//count >= 4 -> special case
	}
}

Edge * IOManager::FindEdgeByStartPoint(int _vIndex)
{
	for (int i = 0; i < l_EdgeDataArray.size(); ++i)
	{
		Edge* pEdge = &l_EdgeDataArray.at(i);
		if (pEdge->vertex1Index == _vIndex || pEdge->vertex2Index == _vIndex)
		{
			return pEdge;
		}
	}
    cout << "ERROR: FIND NO EDGE" <<endl;
	return NULL;
}

Edge * IOManager::FindEdge(int _vStart, int _vEnd)
{
    for (int i = 0; i < l_EdgeDataArray.size(); ++i)
    {
        Edge* pEdge = &l_EdgeDataArray.at(i);
        if ((pEdge->vertex1Index == _vStart && pEdge->vertex2Index == _vEnd)
            ||(pEdge->vertex1Index == _vEnd && pEdge->vertex2Index == _vStart))
        {
            return pEdge;
        }
    }
    cout << "ERROR: FIND NO EDGE" <<endl;
    return NULL;
}

double IOManager::GetLength(Vertex a, Vertex b)
{
    return sqrt((a.x - b.x) * (a.x - b.x)
                +(a.y - b.y) * (a.y - b.y)
                +(a.z - b.z) * (a.z - b.z));
}

void IOManager::GetEdgeDataFromFace(Face* f)
{
	//
	//only consider triangle mesh
	//    
	if(f->numVertex == 3)
	{
		int v1 = f->listVertexIndex.at(0);
		int v2 = f->listVertexIndex.at(1);
		int v3 = f->listVertexIndex.at(2);
		vector<int> temp;
		temp.push_back(v1);
		temp.push_back(v2);
		temp.push_back(v3);
		sort(temp.begin(), temp.end());
        // Each Edge will be member of 2 triangles
        // So we store the 2 other points which form this 2 triangles
        // when find saddle point, we need to know the neighbor edge of and edge
        // this can be form from this 2 points	
		vector<Edge>::iterator it;

        Edge e1 = Edge(temp.at(0), temp.at(1));        
        it = find(l_EdgeDataArray.begin(), l_EdgeDataArray.end(), &e1);
        //duplicate
        if (it != l_EdgeDataArray.end())
        {
            if (find(it->listNeighbor.begin(), it->listNeighbor.end(), temp.at(2)) == it->listNeighbor.end())
            {
                it->listNeighbor.push_back(temp.at(2));
            }

        }
        //no duplicate
        else
        {
            e1.listNeighbor.push_back(temp.at(2));
            l_EdgeDataArray.push_back(e1);
        }
        

		Edge e2 = Edge(temp.at(1), temp.at(2));
        it = find(l_EdgeDataArray.begin(), l_EdgeDataArray.end(), &e2);
        //duplicate
        if (it != l_EdgeDataArray.end())
        {
            if (find(it->listNeighbor.begin(), it->listNeighbor.end(), temp.at(0)) == it->listNeighbor.end())
            {
                it->listNeighbor.push_back(temp.at(0));
            }
        }
        //no duplicate
        else
        {
            e2.listNeighbor.push_back(temp.at(0));
            l_EdgeDataArray.push_back(e2);
        }

		Edge e3 = Edge(temp.at(0), temp.at(2));
        it = find(l_EdgeDataArray.begin(), l_EdgeDataArray.end(), &e3);
        //duplicate
        if (it != l_EdgeDataArray.end())
        {
            
            if (find(it->listNeighbor.begin(), it->listNeighbor.end(), temp.at(1)) == it->listNeighbor.end())
            {
                it->listNeighbor.push_back(temp.at(1));
            }
        }
        //no duplicate
        else
        {
            e3.listNeighbor.push_back(temp.at(1));
            l_EdgeDataArray.push_back(e3);
        }
	}
	else
	{
		cout << "NOT A TRIANGLE MESH" << endl;
	}	
}

void IOManager::CalculateTraceList()
{
    for (int i = 0; i < l_LocalSaddleIndex.size(); ++i)
    {
		//start tracing from 4 boundaries line
		vector<int> traceList;
		traceList.push_back(l_LocalSaddleIndex.at(i));
		//
        Vertex * currSaddle = &l_VertexDataArray.at(l_LocalSaddleIndex.at(i));
        //vector<int> * l_neighborList = &currSaddle->listNeighbor;
        double tempMax = -std::numeric_limits<double>::max();
        double tempMin = std::numeric_limits<double>::max();
        //start with random edge
        Edge * tempStartEdge = FindEdgeByStartPoint(l_LocalSaddleIndex.at(i));
        double val1 = 0;
        if (tempStartEdge->vertex1Index == l_LocalSaddleIndex.at(i))
        {
            val1 = l_VertexDataArray.at(tempStartEdge->vertex1Index).functionValue;
        }
        else
        {
            val1 = l_VertexDataArray.at(tempStartEdge->vertex2Index).functionValue;
        }

        //go to one direction until meet the boundary
		int previous = -1;
        int count = 0;
        while (true)
        {
			int next = -1;
			if (previous == -1 || tempStartEdge->listNeighbor.at(0) == previous)
			{
				next = tempStartEdge->listNeighbor.at(1);
			}
			else
			{
				next = tempStartEdge->listNeighbor.at(0);
			}

			//
			if (tempStartEdge->vertex1Index == l_LocalSaddleIndex.at(i))
			{
				previous = tempStartEdge->vertex2Index;
			} 
			else
			{
				previous = tempStartEdge->vertex1Index;
			}

            double val_compare1 = 0;
			double val_compare2 = 0;
			if (previous != -1)
			{
                double a = l_VertexDataArray.at(previous).functionValue;
                double b = l_VertexDataArray.at(next).functionValue;
				val_compare1 = (l_VertexDataArray.at(next).functionValue - val1);
				val_compare2 = (l_VertexDataArray.at(previous).functionValue - val1);
			}		
			//tracing
			if ((val_compare1 <= 0 && val_compare2 >= 0) || (val_compare1 >= 0 && val_compare2 <= 0))
            {
                TraceLine(previous, traceList, val1);
                count ++;
                if (count == 4) break;
                //continue running to next boundary
                tempStartEdge = FindEdge(l_LocalSaddleIndex.at(i),next);
                //clear trace list
                traceList.clear();
                traceList.push_back(l_LocalSaddleIndex.at(i));
            }
            else
            {
                tempStartEdge = FindEdge(l_LocalSaddleIndex.at(i), next);
            }			
        }	
    }
}

void IOManager::TraceLine(int previous, vector<int> & traceList, double val1)
{
    Vertex * startVertex = &l_VertexDataArray.at(previous);
    //TESTING
    //push the start vertex
    traceList.push_back(previous);
    //from start vertex go up until meet the max or min
    int nextIndex = -1;
    while (!startVertex->isLocalMax && !startVertex->isLocalMin)
    {
        //val1 = local saddle function value
        if (startVertex->functionValue > val1)
        {
            nextIndex = GetMaxNeighbor(previous);
        }
        else
        {
            nextIndex = GetMinNeighbor(previous);
        }
        traceList.push_back(nextIndex);
        startVertex = &l_VertexDataArray.at(nextIndex);
        previous = nextIndex;
    }            
    l_TraceLineList.push_back(traceList);
}

int IOManager::GetMaxNeighbor(int _vIndex)
{
	Vertex * v = &l_VertexDataArray.at(_vIndex);
	double maxVal = -std::numeric_limits<double>::max();
	int result = -1;
	for (int i = 0; i < v->listNeighbor.size(); ++i)
	{
		int nbIndex = v->listNeighbor.at(i);
		if (l_VertexDataArray.at(nbIndex).functionValue > maxVal)
		{
			maxVal = l_VertexDataArray.at(nbIndex).functionValue;
			result = nbIndex;
		}
	}
	return result;
}

int IOManager::GetMinNeighbor(int _vIndex)
{
	Vertex * v = &l_VertexDataArray.at(_vIndex);
	double minVal = std::numeric_limits<double>::max();
	int result = -1;
	for (int i = 0; i < v->listNeighbor.size(); ++i)
	{
		int nbIndex = v->listNeighbor.at(i);
		if (l_VertexDataArray.at(nbIndex).functionValue < minVal)
		{
			minVal = l_VertexDataArray.at(nbIndex).functionValue;
			result = nbIndex;
		}
	}
	return result;
}

void IOManager::SimplifyMesh()
{
	for (int i = 0; i < l_EdgeDataArray.size();++i)
	{
		//find common neighbor of 2 points
		//if only 2 common neighbor -> simplify
		Edge * e = &l_EdgeDataArray.at(i);
		int numCommon = GetNumCommonNeighbor(e->vertex1Index, e->vertex2Index);
		if (numCommon == 2)
		{
			Vertex * v1 = &l_VertexDataArray.at(e->vertex1Index);
			Vertex * v2 = &l_VertexDataArray.at(e->vertex2Index);
			//check if the common list for these 2 vertices exist
			//if not
			if (v1->commonListIndex == -1
				&& v2->commonListIndex == -1)
			{				
				//reset coordinate
				double avg[3];
				avg[0] = (v1->x + v2->x) / 2;
				avg[1] = (v1->y + v2->y) / 2;
				avg[2] = (v1->z + v2->z) / 2;
				if ((avg[0] - v1->x) < ERROR_THRESHOLD && (avg[1] - v1->y) < ERROR_THRESHOLD && (avg[2]-v1->z) < ERROR_THRESHOLD)
				{
					//push to a new list
					vector<int> common;
					common.push_back(e->vertex1Index);
					common.push_back(e->vertex2Index);
					l_commonCoordList.push_back(common);
					//set common index each vertex
					v1->setCommonIndex(l_commonCoordList.size() - 1);
					v2->setCommonIndex(l_commonCoordList.size() - 1);
					v1->setVertexCoord(avg[0], avg[1], avg[2]);
					v2->setVertexCoord(avg[0], avg[1], avg[2]);

				}
			}
		}
	}
}

double * IOManager::CalculateAverageCoord(vector<int> * list)
{
	double sumX = 0, sumY = 0, sumZ = 0;
	for (int i = 0; i < list->size(); ++i)
	{
		Vertex * v = &l_VertexDataArray.at(list->at(i));
		sumX += v->x;
		sumY += v->y;
		sumZ += v->z;
	}
	sumX /= list->size();
	sumY /= list->size();
	sumZ /= list->size();
	double result[3];
	result[0] = sumX;
	result[1] = sumY;
	result[2] = sumZ;
	return result;
}

int IOManager::GetNumCommonNeighbor(int _v1Index, int _v2Index)
{
	Vertex *v1 = &l_VertexDataArray.at(_v1Index);
	Vertex *v2 = &l_VertexDataArray.at(_v2Index);
	vector<int> * l_v1Neighbor = &v1->listNeighbor;
	vector<int> * l_v2Neighbor = &v2->listNeighbor;
	//
	int count = 0;
	for (int i = 0; i < l_v1Neighbor->size(); ++i)
	{
		for (int j = 0; j < l_v2Neighbor->size(); ++j)
		{
			if (l_v1Neighbor->at(i) == l_v2Neighbor->at(j))
			{
				count++;
			}
		}
	}
	return count;
}

void IOManager::SmoothenSurface()
{
	for (int i = 0; i < l_VertexDataArray.size(); ++i)
	{
		Vertex *v = &l_VertexDataArray.at(i);
		if(v->unfoldAngle > 1.5)
		{
			double avg[3];
			avg[0] = avg[1] = avg[2] = 0;
			for (int j = 0; j < v->listNeighbor.size(); ++j)
			{
				Vertex * neighbor = &l_VertexDataArray.at(v->listNeighbor.at(j));
				avg[0] += neighbor->x;
				avg[1] += neighbor->y;
				avg[2] += neighbor->z;
			}
			avg[0] /= v->listNeighbor.size();
			avg[1] /= v->listNeighbor.size();
			avg[2] /= v->listNeighbor.size();
			double error = max(max(abs(v->x -  avg[0]), abs(v->y - avg[1])), abs(v->z - avg[2]));
			if (error < SMOOTH_THRESHOLD)
			{
				v->setVertexCoord((avg[0] + v->x)/2, (avg[1] + v->y)/2, (avg[2]+v->z)/2);
				for (int j = 0; j < v->listNeighbor.size(); ++j)
				{
					Vertex * neighbor = &l_VertexDataArray.at(v->listNeighbor.at(j));
					neighbor->setVertexCoord(avg[0], avg[1], avg[2]);
				}
			}
		}
	}
}

void IOManager::SetFaceNeighborList()
{
	for (int i = 0; i < l_FaceDataArray.size(); ++i)
	{
		Face * f = &l_FaceDataArray.at(i);
		int v1Index = f->listVertexIndex.at(0);
		int v2Index = f->listVertexIndex.at(1);
		int v3Index = f->listVertexIndex.at(2);
		vector<int> list;
		//findNeighbor
		list = GetCommonNeighborWithExclusion(v1Index, v2Index, v3Index);
		for (int j = 0; j < list.size(); ++j)
		{
			int face = FindFace(v1Index, v2Index,list.at(j));
			if (face != -1)
			{
				f->addNeighbor(face);
			}
		}
		//
		list = GetCommonNeighborWithExclusion(v1Index, v3Index, v2Index);
		for (int j = 0; j < list.size(); ++j)
		{
			int face = FindFace(v1Index, v3Index,list.at(j));
			if (face != -1)
			{
				f->addNeighbor(face);
			}
		}
		//
		list = GetCommonNeighborWithExclusion(v3Index, v2Index, v1Index);
		for (int j = 0; j < list.size(); ++j)
		{
			int face = FindFace(v3Index, v2Index,list.at(j));
			if (face != -1)
			{
				f->addNeighbor(face);
			}
		}
	}
}

vector<int> IOManager::GetCommonNeighborWithExclusion(int _v1Index, int _v2Index, int _excludeIndex)
{
	Vertex *v1 = &l_VertexDataArray.at(_v1Index);
	Vertex *v2 = &l_VertexDataArray.at(_v2Index);
	vector<int> * l_v1Neighbor = &v1->listNeighbor;
	vector<int> * l_v2Neighbor = &v2->listNeighbor;
	//
	vector<int> result;
	for (int i = 0; i < l_v1Neighbor->size(); ++i)
	{
		for (int j = 0; j < l_v2Neighbor->size(); ++j)
		{
			if (l_v1Neighbor->at(i) == l_v2Neighbor->at(j))
			{
				if (l_v1Neighbor->at(i) != _excludeIndex)
				{
					result.push_back(l_v1Neighbor->at(i));
				}
			}
		}
	}
	return result;
}

int IOManager::FindFace(int _v1, int _v2, int _v3)
{
	for (int i = 0; i < l_FaceDataArray.size(); ++i)
	{
		Face * f = &l_FaceDataArray.at(i);
		if ((_v1 == f->listVertexIndex.at(0) || _v1 == f->listVertexIndex.at(1) || _v1 == f->listVertexIndex.at(2))
			&& (_v2 == f->listVertexIndex.at(0) || _v2 == f->listVertexIndex.at(1) || _v2 == f->listVertexIndex.at(2))
			&& (_v3 == f->listVertexIndex.at(0) || _v3 == f->listVertexIndex.at(1) || _v3 == f->listVertexIndex.at(2)))
		{
			return i;
		}
	}
	return -1;
}

void IOManager::BuildGraphFromMesh()
{
	for (int i = 0; i < l_FaceDataArray.size(); ++i)
	{
		GraphNode n;
		n.faceId = i;
		faceGraph.push_back(n);
	}
	for (int i = 0; i < faceGraph.size(); ++i)
	{
		GraphNode* n = &faceGraph.at(i);
		//add neighbor
		Face* currFace = &l_FaceDataArray.at(n->faceId);
		for (int j = 0; j < currFace->listNeighbor.size(); j ++)
		{
			int neighborId = currFace->listNeighbor.at(j);
			GraphNode * neighborNode = &faceGraph.at(neighborId);
			n->neighborList.push_back(neighborNode);
		}
	}
}

void IOManager::BreakGraphToTree()
{
	ResetGraph();
	for (int i = 0; i < faceGraph.size(); ++i)
	{
		if (!faceGraph.at(i).visited)
		{
			GraphNode * n = &faceGraph.at(i);
			//process
			//recursively
			ProcessNode(n, NULL);
			DeleteGraphEdge();
		}		
	}
	//VALIDATION
	//COMMENT OUT FOR RELEASE
	//count the number of trees created
	//ResetGraph();
	//int count = 0;
	//bool check = false;
	//for (int i = 0; i < faceGraph.size(); ++i)
	//{
	//	if (!faceGraph.at(i).visited)
	//	{
	//		GraphNode * n = &faceGraph.at(i);
	//		//process
	//		//recursively
	//		check = CheckValidTree(n, NULL);
	//		count ++;
	//	}	
	//}
	//cout << count <<"----"<< check << endl;	
}

void IOManager::ResetGraph()
{
	for (int i = 0; i < faceGraph.size(); ++i)
	{
		faceGraph.at(i).resetVisitedState();
	}
}

void IOManager::ProcessNode(GraphNode * _n, GraphNode * _parent)
{
	_n->visited = true;
	//with each neighbor
	for (int i = 0; i < _n->neighborList.size(); ++i)
	{
		GraphNode *neighbor = _n->neighborList.at(i);
		//
		if (_parent != NULL && neighbor->faceId == _parent->faceId)
		{
			continue;
		}
		GraphEdge e;
		e.setEdge(_n->faceId, neighbor->faceId);
		if(CheckExistedEdge(&e))
		{
			continue;
		}
		//check if it is visited
		if (neighbor->visited)
		{
			//break the edge
			ToDeleteEdgeList.push_back(e);			
		} 
		else
		{
			//recursively call back
			ProcessNode(neighbor, _n);
		}
	}
}

//for VALIDATION
bool IOManager::CheckValidTree(GraphNode * _n, GraphNode * _parent)
{
	_n->visited = true;
	//with each neighbor
	for (int i = 0; i < _n->neighborList.size(); ++i)
	{
		GraphNode *neighbor = _n->neighborList.at(i);
		//
		if (_parent != NULL && neighbor->faceId == _parent->faceId)
		{
			continue;
		}
		GraphEdge e;
		e.setEdge(_n->faceId, neighbor->faceId);
		if(CheckExistedEdge(&e))
		{
			continue;
		}
		//check if it is visited
		if (neighbor->visited)
		{
			return false;		
		} 
		else
		{
			//recursively call back
			CheckValidTree(neighbor, _n);
		}
	}
	return true;
}

bool IOManager::CheckExistedEdge(GraphEdge * e)
{
	for (int j = 0; j < ToDeleteEdgeList.size(); ++j)
	{
		if (e->equals(&ToDeleteEdgeList.at(j)))
		{
			return true;
		}
	}
	return false;
}

void IOManager::DeleteGraphEdge()
{
	for (int i = 0; i < ToDeleteEdgeList.size(); ++i)
	{
		GraphEdge *e = &ToDeleteEdgeList.at(i);
		GraphNode * n1 = &faceGraph.at(e->n1);
		GraphNode * n2 = &faceGraph.at(e->n2);
		for (int j = 0; j < n1->neighborList.size(); ++j)
		{
			if (n2->faceId == n1->neighborList.at(j)->faceId)
			{
				n1->neighborList.erase(n1->neighborList.begin() + j);
				break;
			}
		}
		for (int j = 0; j < n2->neighborList.size(); ++j)
		{
			if (n1->faceId == n2->neighborList.at(j)->faceId)
			{
				n2->neighborList.erase(n2->neighborList.begin() + j);
				break;
			}
		}
	}
}

void IOManager::SegmentGraph()
{
	//From trace list -> create edge
	//then find the 2 node link by this edge
	//unlink nodes
	for (int i = 0; i < l_TraceLineList.size(); ++i)
	{
		vector<int> * ptsList = &l_TraceLineList.at(i);
		for (int j = 0; j < ptsList->size() - 1; ++j)
		{
			int v1 = ptsList->at(j);
			int v2 = ptsList->at(j + 1);
			vector<int> commonList = GetCommonNeighborWithExclusion(v1, v2, -1);
			vector<int> faceList;
			for (int k = 0; k < commonList.size(); ++k)
			{
				int f = FindFace(v1, v2, commonList.at(k));
				if (f != -1)
				{
					faceList.push_back(f);
				}
			}
			if (faceList.size() == 2)
			{
				GraphNode *n1 = &faceGraph.at(faceList.at(0));
				GraphNode *n2 = &faceGraph.at(faceList.at(1));
				for (int k = 0; k < n2->neighborList.size(); ++k)
				{
					if (n2->neighborList.at(k)->faceId == n1->faceId)
					{
						n2->neighborList.erase(n2->neighborList.begin() + k);
						break;
					}
				}
				for (int k = 0; k < n1->neighborList.size(); ++k)
				{
					if (n1->neighborList.at(k)->faceId == n2->faceId)
					{
						n1->neighborList.erase(n1->neighborList.begin() + k);
						break;
					}
				}
			}
		}
	}
}

void IOManager::CalculateUnfoldCoord()
{
	ResetGraph();
	double offset = 0.5;
	//for each Node in graph
	for (int i = 0; i < faceGraph.size(); ++i)
	{
		
		GraphNode * n = &faceGraph.at(i);
		//calculate unfold coordinate
		//TODO
		
		if (!n->visited)
		{
			offset += 1;
			RecursivelyUnfold(n, NULL, offset);
		}
	}
}

void IOManager::RecursivelyUnfold(GraphNode * n, GraphNode * parent, double offset)
{
	n->visited = true;
	//calculate unfold coordinate HERE
	//find coordinate of the first 2 points
	Face * f = &l_FaceDataArray.at(n->faceId);
	Vertex * Orig1 = &l_VertexDataArray.at(f->listVertexIndex.at(0));
	Vertex * Orig2 = &l_VertexDataArray.at(f->listVertexIndex.at(1));
	Vertex * Orig3 = &l_VertexDataArray.at(f->listVertexIndex.at(2));
	
	if (parent == NULL)
	{
		n->unfoldV1.x = n->unfoldV1.y = 0.0;
		n->unfoldV2.x = sqrt((Orig1->x - Orig2->x)*(Orig1->x - Orig2->x)
													  +(Orig1->y - Orig2->y)*(Orig1->y - Orig2->y)
													  +(Orig1->z - Orig2->z)*(Orig1->z - Orig2->z));
		n->unfoldV2.y = 0.0;
		//cal v1 v2 -> v3
		RebuildTriangle(&n->unfoldV1, &n->unfoldV2, &n->unfoldV3,
						NULL, NULL, NULL,
						Orig1, Orig2, Orig3, offset, n);
	}
	else
	{
		//Orig parent
		Face * f = &l_FaceDataArray.at(parent->faceId);
		Vertex * OrigP1 = &l_VertexDataArray.at(f->listVertexIndex.at(0));
		Vertex * OrigP2 = &l_VertexDataArray.at(f->listVertexIndex.at(1));
		Vertex * OrigP3 = &l_VertexDataArray.at(f->listVertexIndex.at(2));

		//orig1, orig2 and origP1, origP2 are common pair
		if (OrigP1 == Orig1 && OrigP2 == Orig2)
		{
			RebuildTriangle(&n->unfoldV1, &n->unfoldV2, &n->unfoldV3,
							&parent->unfoldV1, &parent->unfoldV2, &parent->unfoldV3,
							Orig1, Orig2, Orig3, offset, n);
		} 
		else if (OrigP1 == Orig2 && OrigP2 == Orig1)
		{
			RebuildTriangle(&n->unfoldV1, &n->unfoldV2, &n->unfoldV3,
							&parent->unfoldV2, &parent->unfoldV1, &parent->unfoldV3,
							Orig1, Orig2, Orig3, offset, n);
		}
		//orig1, orig2 and origP1, origP3 are common pair
		else if (OrigP1 == Orig1 && OrigP3 == Orig2)
		{
			RebuildTriangle(&n->unfoldV1, &n->unfoldV2, &n->unfoldV3,
				&parent->unfoldV1, &parent->unfoldV3, &parent->unfoldV2,
				Orig1, Orig2, Orig3, offset, n);
		}
		else if (OrigP1 == Orig2 && OrigP3 == Orig1)
		{
			RebuildTriangle(&n->unfoldV1, &n->unfoldV2, &n->unfoldV3,
				&parent->unfoldV3, &parent->unfoldV1, &parent->unfoldV2,
				Orig1, Orig2, Orig3, offset, n);
		}
		//orig1, orig2 and origP2, origP3 are common pair
		else if (OrigP2 == Orig1 && OrigP3 == Orig2)
		{
			RebuildTriangle(&n->unfoldV1, &n->unfoldV2, &n->unfoldV3,
				&parent->unfoldV2, &parent->unfoldV3, &parent->unfoldV1,
				Orig1, Orig2, Orig3, offset, n);
		}
		else if (OrigP2 == Orig2 && OrigP3 == Orig1)
		{
			RebuildTriangle(&n->unfoldV1, &n->unfoldV2, &n->unfoldV3,
				&parent->unfoldV3, &parent->unfoldV2, &parent->unfoldV1,
				Orig1, Orig2, Orig3, offset, n);
		}
		//ONE THIRD

		//orig1, orig3 and origP1, origP2 are common pair
		if (OrigP1 == Orig1 && OrigP2 == Orig3)
		{
			RebuildTriangle(&n->unfoldV1, &n->unfoldV3, &n->unfoldV2,
				&parent->unfoldV1, &parent->unfoldV2, &parent->unfoldV3,
				Orig1, Orig3, Orig2, offset, n);
		} 
		else if (OrigP1 == Orig3 && OrigP2 == Orig1)
		{
			RebuildTriangle(&n->unfoldV1, &n->unfoldV3, &n->unfoldV2,
				&parent->unfoldV2, &parent->unfoldV1, &parent->unfoldV3,
				Orig1, Orig3, Orig2, offset, n);
		}
		//orig1, orig3 and origP1, origP3 are common pair
		else if (OrigP1 == Orig1 && OrigP3 == Orig3)
		{
			RebuildTriangle(&n->unfoldV1, &n->unfoldV3, &n->unfoldV2,
				&parent->unfoldV1, &parent->unfoldV3, &parent->unfoldV2,
				Orig1, Orig3, Orig2, offset, n);
		}
		else if (OrigP1 == Orig3 && OrigP3 == Orig1)
		{
			RebuildTriangle(&n->unfoldV1, &n->unfoldV3, &n->unfoldV2,
				&parent->unfoldV3, &parent->unfoldV1, &parent->unfoldV2,
				Orig1, Orig3, Orig2, offset, n);
		}
		//orig1, orig3 and origP2, origP3 are common pair
		else if (OrigP2 == Orig1 && OrigP3 == Orig3)
		{
			RebuildTriangle(&n->unfoldV1, &n->unfoldV3, &n->unfoldV2,
				&parent->unfoldV2, &parent->unfoldV3, &parent->unfoldV1,
				Orig1, Orig3, Orig2, offset, n);
		}
		else if (OrigP2 == Orig3 && OrigP3 == Orig1)
		{
			RebuildTriangle(&n->unfoldV1, &n->unfoldV3, &n->unfoldV2,
				&parent->unfoldV3, &parent->unfoldV2, &parent->unfoldV1,
				Orig1, Orig3, Orig2, offset, n);
		}
		//TWO THIRD
		
		//orig2, orig3 and origP1, origP2 are common pair
		if (OrigP1 == Orig2 && OrigP2 == Orig3)
		{
			RebuildTriangle(&n->unfoldV2, &n->unfoldV3, &n->unfoldV1,
				&parent->unfoldV1, &parent->unfoldV2, &parent->unfoldV3,
				Orig2, Orig3, Orig1, offset, n);
		} 
		else if (OrigP1 == Orig3 && OrigP2 == Orig2)
		{
			RebuildTriangle(&n->unfoldV2, &n->unfoldV3, &n->unfoldV1,
				&parent->unfoldV2, &parent->unfoldV1, &parent->unfoldV3,
				Orig2, Orig3, Orig1, offset, n);
		}
		//orig2, orig3 and origP1, origP3 are common pair
		else if (OrigP1 == Orig2 && OrigP3 == Orig3)
		{
			RebuildTriangle(&n->unfoldV2, &n->unfoldV3, &n->unfoldV1,
				&parent->unfoldV1, &parent->unfoldV3, &parent->unfoldV2,
				Orig2, Orig3, Orig1, offset, n);
		}
		else if (OrigP1 == Orig3 && OrigP3 == Orig2)
		{
			RebuildTriangle(&n->unfoldV2, &n->unfoldV3, &n->unfoldV1,
				&parent->unfoldV3, &parent->unfoldV1, &parent->unfoldV2,
				Orig2, Orig3, Orig1, offset, n);
		}
		//orig2, orig3 and origP2, origP3 are common pair
		else if (OrigP2 == Orig2 && OrigP3 == Orig3)
		{
			RebuildTriangle(&n->unfoldV2, &n->unfoldV3, &n->unfoldV1,
				&parent->unfoldV2, &parent->unfoldV3, &parent->unfoldV1,
				Orig2, Orig3, Orig1, offset, n);
		}
		else if (OrigP2 == Orig3 && OrigP3 == Orig2)
		{
			RebuildTriangle(&n->unfoldV2, &n->unfoldV3, &n->unfoldV1,
				&parent->unfoldV3, &parent->unfoldV2, &parent->unfoldV1,
				Orig2, Orig3, Orig1, offset, n);
		}
	}

	//process recursively neighbor HERE
	for (int i = 0; i < n->neighborList.size(); ++i)
	{
		GraphNode *neighbor = n->neighborList.at(i);
		if (parent != NULL && parent->faceId ==neighbor->faceId)
		{
			continue;
		}
		//
		if (!neighbor->visited)
		{
			RecursivelyUnfold(neighbor, n, offset);
		}
	}
}


void IOManager::RebuildTriangle(Vertex * unfoldV1, Vertex * unfoldV2, Vertex * unfoldV3, 
								Vertex * parentU1,Vertex * parentU2,Vertex * parentU3,
								Vertex * origN1, Vertex * origN2,Vertex * origN3
								,double offset, GraphNode * n)
{
	double rA, rB, xA, xB, yA, yB, k, dSquare;
	//
	
	//
	if (parentU1 != NULL && parentU2 != NULL && parentU3 != NULL)
	{
		xA = unfoldV1->x = parentU1->x;
		yA = unfoldV1->y = parentU1->y;
		xB = unfoldV2->x = parentU2->x;
		yB = unfoldV2->y = parentU2->y;
	}
	else
	{
		xA = unfoldV1->x;
		yA = unfoldV1->y;
		xB = unfoldV2->x;
		yB = unfoldV2->y;
	}
	rA = sqrt((origN3->x - origN1->x)*(origN3->x - origN1->x)
		    + (origN3->y - origN1->y)*(origN3->y - origN1->y)
			+ (origN3->z - origN1->z)*(origN3->z - origN1->z));
	rB = sqrt((origN3->x - origN2->x)*(origN3->x - origN2->x)
		+ (origN3->y - origN2->y)*(origN3->y - origN2->y)
		+ (origN3->z - origN2->z)*(origN3->z - origN2->z));
	
	//
	/*dSquare = (xB - xA)*(xB - xA) + (yB - yA)*(yB - yA);
	k = 0.25 * sqrt(((rA + rB)*(rA + rB) - dSquare)*(dSquare - (rA - rB)*(rA - rB)));*/

	double e = xB - xA;
	double f = yB - yA;
	double p = sqrt(e*e + f*f);
	k = (p*p + rA*rA - rB*rB) / (2*p);

	//
	double x1, y1, x2, y2;
	x1 = xA + e*k/p + (f/p)*sqrt(rA*rA - k*k);
	y1 = yA + f*k/p - (e/p)*sqrt(rA*rA - k*k);
	x2 = xA + e*k/p - (f/p)*sqrt(rA*rA - k*k);
	y2 = yA + f*k/p + (e/p)*sqrt(rA*rA - k*k);
	/*x1 = 0.5 * (xB + xA) + 0.5 * (xB - xA)*(rA*rA - rB*rB)/dSquare + 2 * (yB - yA)*k/dSquare;
	y1 = 0.5 * (yB + yA) + 0.5 * (yB - yA)*(rA*rA - rB*rB)/dSquare - 2 * (xB - xA)*k/dSquare;

	x2 = 0.5 * (xB + xA) + 0.5 * (xB - xA)*(rA*rA - rB*rB)/dSquare - 2 * (yB - yA)*k/dSquare;
	y2 = 0.5 * (yB + yA) + 0.5 * (yB - yA)*(rA*rA - rB*rB)/dSquare + 2 * (xB - xA)*k/dSquare;*/

	if (parentU1 == NULL || parentU2 == NULL || parentU3 == NULL)
	{
			unfoldV3->x = x1;
			unfoldV3->y = y1;
	} 
	else
	{
		double d1, d2;

		d1 = (x1 - parentU3->x)*(x1 - parentU3->x) + (y1 - parentU3->y)*(y1 - parentU3->y);
		d2 = (x2 - parentU3->x)*(x2 - parentU3->x) + (y2 - parentU3->y)*(y2 - parentU3->y);

		if (d1 >= d2)
		{
			unfoldV3->x = x1;
			unfoldV3->y = y1;
		}
		else
		{
			unfoldV3->x = x2;
			unfoldV3->y = y2;
		}
	}
	n->offset = offset;
}